/*****************************************************
Chip type               : ATmega32
Program type            : Application
AVR Core Clock frequency: 7,3728 MHz
Memory model            : Small
External RAM size       : 0
Data Stack size         : 512
*****************************************************/
/*****************************************************
Assumed hardware connections:

PortA:
    PA0 - 1-st channel RSSI input
    PA1 - 2-nd channel RSSI input
    PA2 - 3-rd channel RSSI input
    PA3 - 4-th channel RSSI input
    PA4 - set switching hysteresis
    PA5..7 - not used at this moment

PortB:
    PB0 - 1-st channel control output (GND to switch on)
    PB1 - 2-nd channel control output (GND to switch on)    
    PB2 - 3-rd channel control output (GND to switch on)
    PB3 - 4-th channel control output (GND to switch on)
    PB4 - main status LED (GND to switch on)
    PB5 - configuration #1 active (GND to switch on)
    PB6 - configuration #2 active (GND to switch on)
    PB7 - configuration #3 active (GND to switch on)
    
PortC:
    PC0 - set 1-st channel Rx RSSI type - GND for Lawmate, VCC for chinese
    PC1 - set 2-st channel Rx RSSI type - GND for Lawmate, VCC for chinese
    PC2 - set 3-st channel Rx RSSI type - GND for Lawmate, VCC for chinese
    PC3 - set 4-st channel Rx RSSI type - GND for Lawmate, VCC for chinese
    PC4 - enter setup mode after power on
    PC5 - choose Configuration #1
    PC6 - choose Configuration #2
    PC7 - choose Configuration #3
    
    
PortD:
    PD0 - USART Rx
    PD1 - USART Tx
    PD2 - Write button (writes settings to EEPROM in setup mode) 
    PD3..7 - not used at this moment        
*****************************************************/
#include <mega32.h>
#include <delay.h>
#include <stdio.h>

//defRx - sets Rx RSSI type (0 - normal, 1 - inverted) at each channel
//rxrssi contains RSSI voltages, currentADC points to current processing ADC channel
//hyst sets hysteresis for switching channels, currentRx is currently selected VideoRx
//rxinuse contains number of currently connected VideoRxs (diversity would use only this number of ports starting from VIN1)  
unsigned char rxrssi[4],currentADC=0,currentRx=0,defRx[4],hyst,rxinuse,usartRxEn,usartTxEn;



// RSSI Update function
interrupt [17] void rssiupdate(void) {
rxrssi[currentADC]=ADCH; 
//switch to next ADC input
if (currentADC==3) currentADC=0; else currentADC++;
ADMUX=(ADMUX&252)^currentADC;
//start next ADC sequence
ADCSRA.6=1;
} 



//function for reading byte from EEPROM
unsigned char eeread (unsigned int uiAddress) {
//wait for completion of previous write if any
while (EECR&(1<<EEWE)) {};
//set up address register
EEAR=uiAddress;
//start read by writing EERE and return data
EECR|= (1<<EERE);
return EEDR;
}



//function for writing byte to EEPROM
void eewrite (unsigned int uiAddress, unsigned char ucData) {
//wait for completion of previous write if any
while (EECR&(1<<EEWE)) {};
//set up address and data registers
EEAR=uiAddress;
EEDR=ucData;
//set EEMWE and then start write by setting EEWE
EECR|=(1<<EEMWE);
EECR|=(1<<EEWE);
}


// Function for choosing VideoRx with best signal at this moment
//simple switching logic - choosing best RSSI, keeping last choosen channel if all RSSI are equal
//and taking into account Rx settings (normal/reversed RSSI, hysteresis)
unsigned char rxchoose(void) {
unsigned char i,normrssi[4],proposedRx=0;
//normalizing RSSI
for (i=0;i<=3;i++) if (defRx[i]) normrssi[i]=255-rxrssi[i]; else normrssi[i]=rxrssi[i];

//Choosing Rx with best signal 
for (i=0;i<=3;i++) if (normrssi[i]>normrssi[proposedRx]) proposedRx=i; 

//Decide if switching needed: compare RSSI difference and hysteresis
if (normrssi[proposedRx]>normrssi[currentRx]+hyst) currentRx=proposedRx;


//Sending data via USART, used to log data with computer
if (usartTxEn) printf("RSSI: %03d; %03d; %03d; %03d. Using Rx %d\n",normrssi[0],normrssi[1],normrssi[2],normrssi[3],currentRx+1);

//assembling and returning result
switch (currentRx) {
    case 0:return (PORTB&240)^14; break; //change bit3..0 to 1110
    case 1:return (PORTB&240)^13; break; //change bit3..0 to 1101
    case 2:return (PORTB&240)^11; break; //change bit3..0 to 1011
    case 3:return (PORTB&240)^7;  break; //change bit3..0 to 0111
    };
// Next line should work only in case something goes wrong...    
return PORTB;    
}



//Setup mode function, used when setup trigger is enabled
//All settings are written to EEPROM to the corresponding configuration section 
//Byte 0-1 Config#1, Byte 2-3 Config#2, Byte 4-5 Config#3
//Lower byte contains settings, higher byte contains hysteresis.
//Lower byte bits (7..0): Rx0type, Rx1type, Rx2type, Rx3type, #Rx Used (2 bits), USART Rx Enable, USART Tx Enable
void setupmode (void){
unsigned char i=0,getconfnumb=0;

//wait until only one config is selected, poka-yoke (mistake-proofing)
getconfnumb=PINC&224;
while (!((getconfnumb==192)||(getconfnumb==160)||(getconfnumb==96))) {
PORTB.5=~PORTB.5;PORTB.6=~PORTB.6;PORTB.7=~PORTB.7;
delay_ms(100);
getconfnumb=PINC&224;
};

//switch on selected config LED
PORTB=(PORTB&31)^getconfnumb;

// the only way out from setup mode is reset!
while (1) {
    /* Read and modify running config section */
    //Read Rx settings from PortC0..3
    for (i=0;i<=3;i++) defRx[i]=(PINC>>(i))&1;
    //Read PortA5 and set hysteresis value
    while(!ADCSRA.4) {}; ADMUX=0b01100100; ADCSRA.6=1; 
    while(!ADCSRA.4) {}; hyst=ADCH/4;
    //Read RSSI from all ports
    ADMUX=0b01100000; ADCSRA.6=1; while(!ADCSRA.4) {}; rxrssi[0]=ADCH;          
    ADMUX=0b01100001; ADCSRA.6=1; while(!ADCSRA.4) {}; rxrssi[1]=ADCH;          
    ADMUX=0b01100010; ADCSRA.6=1; while(!ADCSRA.4) {}; rxrssi[2]=ADCH;          
    ADMUX=0b01100011; ADCSRA.6=1; while(!ADCSRA.4) {}; rxrssi[3]=ADCH;
    
    PORTB=rxchoose();
    //Sending data via USART, used to log data with computer
    if (usartTxEn) printf("Setup mode. Rx type: %d; %d; %d; %d. Hysteresis: %02d\n",defRx[0],defRx[1],defRx[2],defRx[3],hyst);

    /* Write settings to EEPROM if write key is pressed*/
    if (!PIND.3) {
        for (i=0;i<10;i++) {PORTB^=getconfnumb; delay_ms(10);}; //blink config LED
        //assembling first byte
        i=((defRx[0]<<7)|(defRx[1]<<6)|(defRx[2]<<5)|(defRx[3]<<4)|(rxinuse<<2)|(usartRxEn<<1)|(usartTxEn));
        switch (getconfnumb) {
            case 192: eewrite(0,i); eewrite(1,hyst^128); break;
            case 160: eewrite(2,i); eewrite(3,hyst^128); break;
            case 96:  eewrite(4,i); eewrite(5,hyst^128); break;
            };         
        };
                   
    //flashing status LED    
    PORTB.4=~PORTB.4;
    }
}

    

//Load config from EEPROM
void loadconfig (void) {
unsigned char getconfnumb=0,tmp=0,i=0;

//wait until only one config is selected, poka-yoke (mistake-proofing)
getconfnumb=PINC&224;
while (!((getconfnumb==192)||(getconfnumb==160)||(getconfnumb==96))) {
PORTB.5=~PORTB.5;PORTB.6=~PORTB.6;PORTB.7=~PORTB.7;
delay_ms(100);
getconfnumb=PINC&224;
};

//switch on selected config LED and load config data from EEPROM, if selected config has not been set then switch to setup mode 
switch (getconfnumb) {
    case 192: PORTB=(PORTB&31)^192; tmp=eeread(0); hyst=eeread(1); if ((hyst&192)!=128) setupmode(); else hyst&=63; for (i=0;i<=3;i++) defRx[i]=(tmp>>(7-i))&1; rxinuse=(tmp>>2)&3; usartRxEn=(tmp>>1)&1; usartTxEn=tmp&1; break;
    case 160: PORTB=(PORTB&31)^160; tmp=eeread(2); hyst=eeread(3); if ((hyst&192)!=128) setupmode(); else hyst&=63; for (i=0;i<=3;i++) defRx[i]=(tmp>>(7-i))&1; rxinuse=(tmp>>2)&3; usartRxEn=(tmp>>1)&1; usartTxEn=tmp&1; break;
    case 96:  PORTB=(PORTB&31)^96;  tmp=eeread(4); hyst=eeread(5); if ((hyst&192)!=128) setupmode(); else hyst&=63; for (i=0;i<=3;i++) defRx[i]=(tmp>>(7-i))&1; rxinuse=(tmp>>2)&3; usartRxEn=(tmp>>1)&1; usartTxEn=tmp&1; break;
    };

}



void init_avr(void){

// Timer/Counters initialization
TCCR0=0x00;
TCNT0=0x00;
OCR0=0x00;
TCCR1A=0x00;
TCCR1B=0x00;
TCNT1H=0x00;
TCNT1L=0x00;
ICR1H=0x00;
ICR1L=0x00;
OCR1AH=0x00;
OCR1AL=0x00;
OCR1BH=0x00;
OCR1BL=0x00;
ASSR=0x00;
TCCR2=0x00;
TCNT2=0x00;
OCR2=0x00;
TIMSK=0x00;

// External Interrupts disabled
MCUCR=0x00;
MCUCSR=0x00;

//Ports configuration - PortA as input without pullup, PortB as output, PortC and PortD as inputs with internal pullup (except PORTD.0 and PORTD.1).
PORTA=0x00;
DDRA=0x00;
PORTB=0xFF;
DDRB=0xFF;
PORTC=0xFF;
DDRC=0x00;
PORTD=0xFC;
DDRD=0x00;

//ADC Initialization: set AVCC as reference voltage, use left alignment of result, choose portA.pin4 as ADC source
//Enable ADC, start first conversion (PortA.0), disable Auto Triggering of ADC, enable ADC Conversion Complete Interrupt and set ADC clock as 1/64 of the XTAL 
ADMUX=0b01100000;
ADCSRA=0b11001110;

// Analog Comparator: Off, ADC mode: Free Running
// Analog Comparator Input Capture by Timer/Counter 1: Off
ACSR=0x80;
SFIOR=0x00;

//USART init: 8n1 (8 data, no parity, 1 stop)
//Rx off, Tx on, asyncronous, 115200 baud
UCSRA=0x00;
UCSRC=0x86;
UBRRH=0x00;
UBRRL=0x03;
UCSRB=0x08;

//Choose 1-st Rx channel at startup (just because it's first) and switch main status LED on
PORTB=0b11101110;
}



//Self-test procedure, light up all LEDs, then cycle through all channels
void selftest(void) {
unsigned char i=0;
//switch main status LED on, choose first Rx channel
PORTB=0b11101110;
//cycle through all 4 channels while flashing main status LED 
for (i=0;i<10;i++) { PORTB.4=~PORTB.4; delay_ms(100); };
PORTB=(PORTB&240)^13;
for (i=0;i<10;i++) { PORTB.4=~PORTB.4; delay_ms(100); };
PORTB=(PORTB&240)^11;
for (i=0;i<10;i++) { PORTB.4=~PORTB.4; delay_ms(100); };
PORTB=(PORTB&240)^7;
for (i=0;i<10;i++) { PORTB.4=~PORTB.4; delay_ms(100); };
    
PORTB=0b11101110;    
}

void main(void)
{

init_avr();
//after enabling WatchDog timer add "IF NO WATCHDOG RESET OCCURED RECENTLY" to selftest function call 
selftest();

if (!PINC.4) setupmode(); else loadconfig();

//Global enable interrupts
#asm("sei")

while (1)
      {
       PORTB=rxchoose();
       
       //REMOVE THIS DELAY BEFORE FLASHING REAL DEVERSITY MODULE      
       delay_ms(100);      
      }
}
